%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Edited by Shu Jiang, 2011.
% ALL RIGHTS ARE RESERVED.
% mailto: shujiang@tamu.edu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculate the slope from the hip to swing leg ankle
function ret = Knee3D_slope(xpos,ypos,zpos,sns_brk,StanceLegIndex)
%     humanData = getData(name,i);

StartPt = sns_brk(2);
EndPt = sns_brk(3);
% from heel strike to the other heel strike
%%

for i = 1:length(xpos.lknee)
    
    % transformation
        model_rhip = Data2Model(xpos.rhip(i),ypos.rhip(i),zpos.rhip(i));
        model_lhip = Data2Model(xpos.lhip(i),ypos.lhip(i),zpos.lhip(i));
        model_rankle_avg = Data2Model(xpos.rankle_avg(i),ypos.rankle_avg(i),zpos.rankle_avg(i));
        model_lankle_avg= Data2Model(xpos.lankle_avg(i),ypos.lankle_avg(i),zpos.lankle_avg(i));
     model_hip_avg = Data2Model(xpos.hip_avg(i),ypos.hip_avg(i),zpos.hip_avg(i));
    % right leg is the stance leg
    if StanceLegIndex == 1      
        % stance slope      
        s_slope(i) = StanceSlope(model_rhip(2),model_rhip(3),...
            model_rankle_avg(2), model_rankle_avg(3));
        % non-stance slope
        ns_slope(i) = NonStanceSlope(model_lhip(2),model_lhip(3),...
            model_lankle_avg(2), model_lankle_avg(3));
        hip_ypos(i) = model_hip_avg(2);
        % position on y-direction
        ship_ypos(i) = model_rhip(2);
        nship_ypos(i) = model_lhip(2);
        nsa_ypos(i) = model_lankle_avg(2);
    else
        % stance slope
        s_slope(i) = StanceSlope(model_lhip(2),model_lhip(3),...
            model_lankle_avg(2),model_lankle_avg(3));
        % non-stance slope
        ns_slope(i) = NonStanceSlope(model_rhip(2), model_rhip(3),...
            model_rankle_avg(2), model_rankle_avg(3));
        
        % position on y-direction
        hip_ypos(i) = -model_hip_avg(2);
        ship_ypos(i) = -model_lhip(2);
        nship_ypos(i) = -model_rhip(2);
        nsa_ypos(i) = -model_rankle_avg(2);
        
    end
    
end

ret.nsslope = ns_slope(StartPt:EndPt);
ret.sslope = s_slope(StartPt:EndPt);
ret.hipy = hip_ypos(StartPt:EndPt);
% position on y-direction
ret.shipy = ship_ypos(StartPt:EndPt)-ship_ypos(StartPt);
ret.nshipy = nship_ypos(StartPt:EndPt)-nship_ypos(StartPt);
ret.nsay = nsa_ypos(StartPt:EndPt)-nsa_ypos(StartPt);
% plot(ret.sslope); hold on;
end

% use the angle_avg
function ret = NonStanceSlope(yhip,zhip,yankle,zankle)
ret =   -(yhip-yankle)/(zhip-zankle);
end

% use the angle_avg
function ret = StanceSlope(yhip,zhip,yankle,zankle)
ret =   (yhip-yankle)/(zhip-zankle);
end

